
#include "test_redar.h"

bool RadarDataReader::load(std::string file)
{
    // check file exist
    std::string str_one_line;
    std::ifstream myfile(file);

    int a = 1;

    //int a = 1;

    if(myfile.eof())
    {
        cout<<"Input error!"<<endl;
        return false;
    }
    // while, read one line each time
    while (getline(myfile, str_one_line))
    {

        cout<<a++<<endl;
        if(a == 100)
            break;

        //cout<<a++<<endl;

        if(!getDataOneLine(str_one_line, vec_frm))
        {
            cout<<"getDataOneLine error!"<<endl;
            //return false;
        }
    }
    for(int i = 0; i < DATABOUND; i++)
    {
        double num;
        num = -5 + 0.5 * i;
        vec_ange.push_back(num);
    }
    return true;
}

bool RadarDataReader::getDataOneLine(
        string &str_one_line,
        std::vector<RadarDataReader::RadarDataFrm> &vec_frm)
{
    RadarDataFrm temp;
    char num[DAYBOUND];
    int i = 0;
    int lon = str_one_line.size();
    int locat = 0;
    int gold = 0;
    static int count = 0;

    //Find real time;
    while (str_one_line[locat] != '\t')
    {
        num[i++] = str_one_line[locat];
        ++locat;
    }
    num[i] = '\0';
    temp.time = QDateTime::fromString(num,"yyyy-MM-dd hh:mm:ss.zzz");
    //QString  str = (temp.time).toString("yyyy-MM-dd hh:mm:ss.zzz");
    //qDebug()<<str<<endl;
    while('\t' != str_one_line[locat + 1])
    {
        locat++;
    }
    temp.line = str_one_line[locat] - '0';

    if(lon < 1000) // find begin time;
    {
        begintime = temp.time;
        return true;
    }

    while (lon > locat + 9 && lon > 1000)
    {
        if(vec_frm.size() > 0)
        {
            if(temp.line == vec_frm[vec_frm.size() - 1].line)
            {
                count++;
            }
            else
            {
                int cha;
                int left;
                cha = vec_frm[vec_frm.size() - count].time.msecsTo(temp.time);
                if(cha >= count)
                {
                    left = cha / count;
                    for(int j = 0; j < count; j++)
                    {
                        int addtime;
                        addtime = (count - j - 1) * left;
                        vec_frm[vec_frm.size() - j - 1].time = vec_frm[vec_frm.size() - j - 1].time.addMSecs(addtime);
                        //QString  str = vec_frm[vec_frm.size() - 1 -j].time.toString("yyyy-MM-dd hh:mm:ss.zzz");
                        //qDebug()<<str<<endl;
                    }
                }
                count = 1;
            }
        }
        else
            count++;


        while ('<' == str_one_line[locat]) //Find 00000000
            ++locat;
        ++locat;

        while (lon > locat)
        {
            while (lon > locat)
            {
                if ('3' == str_one_line[locat])
                {
                    ++locat;
                    if ('0' == str_one_line[locat])
                    {
                        ++locat;
                        ++gold;
                    }
                }
                else if ('<' == str_one_line[locat] || '>' == str_one_line[locat])
                {
                    ++locat;
                    continue;
                }
                else
                {
                    ++locat;
                    break;
                }
            }
            if (gold == 8)
                break;
            else
                gold = 0;
        }
        if ('3' != str_one_line[locat - 69] || '4' != str_one_line[locat - 68]) //Find 40000000
        {
            cout << "Cannot found 40000000, error!" << endl;
            return false;
        }

        locat = locat + 3;
        int bound = locat + 16;
        gold = 0;
        while (locat < bound)
        {
            if ('<' == str_one_line[locat] || '>' == str_one_line[locat])
            {
                locat++;
            }
            else if ('4' == str_one_line[locat] && '6' == str_one_line[locat + 1])
            {
                ++gold;
                locat += 2;
            }
            else
                break;
        }
        if (4 != gold)
        {
            cout << "Begin angel error!" << endl;
            return false;
        }


        gold = 0;
        while (TRUE)
        {
            locat++;
            if ('2' == str_one_line[locat] && '0' == str_one_line[locat + 1])
                gold++;
            if (2 == gold)
                break;
        }

        locat += 4;
        if ('3' != str_one_line[locat] || '1' != str_one_line[locat + 1] || '3' != str_one_line[locat + 4] || '7' != str_one_line[locat + 5] || '4' != str_one_line[locat + 8] || '4' != str_one_line[locat + 9])  //Find 17D
        {
            cout << "Data input error!" << endl;
            return false;
        }

        locat += 16;
        std::vector<int> tempdata;
        for (int i = 0; i < DATABOUND; i++)
        {
            tempdata.push_back(CharToInt(str_one_line, locat));
            locat += 4;
        }
        temp.vec_dis = tempdata;

        while (locat + 9 < lon)
        {
            if ('3' == str_one_line[locat] && '0' == str_one_line[locat + 1] && '0' == str_one_line[locat + 4] && '3' == str_one_line[locat + 5] && '0' == str_one_line[locat + 8] && '2' == str_one_line[locat + 9])
            {
                locat += 8;
                break;
            }
            else
                ++locat;
        }
        vec_frm.push_back(temp);
    }

    return true;

}

int RadarDataReader::CharToInt(string &s, int &locat)
{
    int bound;
    int sum = 0;
    int ret = 0;
    int i;
    int lon = s.size();

    if (locat + 12 > lon || ('2' == s[locat + 8] && '0' == s[locat + 9]))
    {
        i = 1;
        bound = locat + 8;
    }
    else
    {
        i = 2;
        bound = locat + 12;
    }

    while (locat < bound && locat + 2 < lon)
    {
        if ('<' == s[locat] || '>' == s[locat])
            ++locat;
        else
        {
            sum = (s[locat++] - '0') * 10;
            sum += s[locat++] - '0';
            if (sum < 40)
                sum -= 30;
            else
                sum -= 31;
            ret += sum * pow(16, i--);
        }
    }
    return ret;
}

bool RadarDataReader::SaveData()
{
    QDateTime nowtime = QDateTime::currentDateTime();
    QString str = nowtime.toString("yyyymmddhhmmsszzz");
    str += ".txt";
    QByteArray temp = str.toLatin1();
    char *buff = temp.data();
    FILE *RadarFile;
    RadarFile = fopen(buff,"w");
    if(RadarFile == NULL)
    {
        cout << "Creat RadarFile error!" << endl;
        return false;
    }
    int lon = vec_ange.size();
    fprintf(RadarFile,"%s\n","Angels:");
    for(int i = 0; i < lon; i++)
    {
        fprintf(RadarFile,"%f\n",vec_ange[i]);
    }
    int frmlon = vec_frm.size();
    for(int i = 0; i < frmlon; i++)
    {
        fprintf(RadarFile,"%s:", "time");
        str = vec_frm[i].time.toString("yyyy-MM-dd hh:mm:ss.zzz");
        temp = str.toLatin1();
        buff = temp.data();
        fprintf(RadarFile,"%s\n", buff);
        for(int j = 0; j < lon; j++)
        {
            fprintf(RadarFile,"%d\n", vec_frm[i].vec_dis[j]);
        }
    }
    fclose(RadarFile);
    return true;
}


bool RadarDataReader::To3DLocation()
{
    int lon = vec_frm.size();
    if(!lon)
    {
        cout << "Data is empty!" << endl;
        return false;
    }
    QDateTime nowtime = QDateTime::currentDateTime();
    QString str = nowtime.toString("yyyymmddhhmmsszzz");
    str += ".pcd";
    QByteArray temp = str.toLatin1();
    char *buff = temp.data();
    FILE *RadarFile;
    RadarFile = fopen(buff,"w");
    if(RadarFile == NULL)
    {
        cout << "Creat RadarFile error!" << endl;
        return false;
    }

    fprintf(RadarFile,"%s\n", "# .PCD v.7 - Point Cloud Data file format  ");
    fprintf(RadarFile,"%s\n", "VERSION .7 ");
    fprintf(RadarFile,"%s\n", "FIELDS x y z rgb  ");
    fprintf(RadarFile,"%s\n", "SIZE 4 4 4 4  ");
    fprintf(RadarFile,"%s\n", "TYPE F F F F  ");
    fprintf(RadarFile,"%s\n", "COUNT 1 1 1 1  ");
    fprintf(RadarFile,"%s", "WIDTH ");
    fprintf(RadarFile,"%d \n", lon*DATABOUND);
    fprintf(RadarFile,"%s\n", "HEIGHT 1  ");
    fprintf(RadarFile,"%s\n", "VIEWPOINT 0 0 0 1 0 0 0  ");
    fprintf(RadarFile,"%s", "POINTS ");
    fprintf(RadarFile,"%d \n", lon*DATABOUND);
    fprintf(RadarFile,"%s\n", "DATA ascii  ");

    int x, y, z;
    for(int i = 0; i < (lon); i++)
    {
        for(int j = 0; j < DATABOUND; j++)
        {
            x = vec_frm[i].vec_dis[j] * cos(vec_ange[j] * PI / 180);
            y = vec_frm[i].vec_dis[j] * sin(vec_ange[j] * PI / 180);
            z = begintime.msecsTo(vec_frm[i].time);

            fprintf(RadarFile,"%d ", x);
            fprintf(RadarFile,"%d ", y);
            fprintf(RadarFile,"%d ", z);
            if(vec_frm[i].vec_dis[j] < 20)                  //底层噪音去除
                fprintf(RadarFile,"%s\n", "1.2108e+06  ");
            else if(vec_frm[i].vec_dis[j] < 120)            //补光灯特征
                fprintf(RadarFile,"%s\n", "8.2108e+06  ");
            else if(vec_frm[i].vec_dis[j] < 170)            //接触线特征
                fprintf(RadarFile,"%s\n", "12.2108e+06  ");
            else if(vec_frm[i].vec_dis[j] < 300)            //接触线上方噪声
                fprintf(RadarFile,"%s\n", "12.2108e+06  ");
            else if(vec_frm[i].vec_dis[j] > 800)            //外部噪声
                fprintf(RadarFile,"%s\n", "16.2108e+06  ");
            else                                            //弧面内侧
                fprintf(RadarFile,"%s\n", "4.2108e+06  ");
           /* if(vec_frm[i].vec_dis[j] > 120 && vec_frm[i].vec_dis[j] < 170)
            {
                fprintf(RadarFile,"%d ", x);
                fprintf(RadarFile,"%d ", y);
                fprintf(RadarFile,"%d\n", z);
                //fprintf(RadarFile,"%s\n", "16.2108e+06  ");
            }*/
        }
    }
    fclose(RadarFile);
    return true;
}

